﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Xml;

namespace Simulador_Quadrotor
{
    [Serializable]
    public class GainScheduling : List<GainPID>
    {

        public ConfiguracionPurebaPID DamePID(int rango)
        {
            List<int> rangos = new List<int>();
            this.ForEach(x => rangos.Add(x.Key));

            //si esta el del rango se lo devolvemos
            if (rangos.Contains(rango))
            {
                return this.DameValor(rango);
            }
            //si no buscamos el mas cercano 
            else
            {
                rangos.Add(rango);
                rangos.Sort();
                int posicion = rangos.IndexOf(rango);
                if (rangos.Count > 2)
                {
                    //si esta el primero de volvemos el segundo
                    if (posicion == 0)
                    {
                        return this.ElementAt(1).Value;
                    }
                    //si esta el ultimo devlvemos el penultimo
                    if (posicion == rangos.Count - 1)
                    {
                        return this.ElementAt(posicion - 1).Value;
                    }
                    //si esta entre dos calculamos el mas cercano
                    int cercano1 = rangos.ElementAt(posicion - 1);
                    int cercano2 = rangos.ElementAt(posicion + 1);
                    int comp1 = rango - cercano1;
                    int comp2 = cercano2 - rango;
                    if (comp1 < comp2)
                    {
                        return this.DameValor(cercano1);
                    }
                    if (comp1 > comp2)
                    {
                        return this.DameValor(cercano2);
                    }
                    //si son iguales devuelvo el de abajo
                    return this.DameValor(cercano1);
                }
                return this.ElementAt(0).Value;
            }
        }

        public ConfiguracionPurebaPID DameValor(int key)
        {
            for (int i = 0; i < this.Count; i++)
            {
                if (this[i].Key == key)
                {
                    return this[i].Value;
                }
            }
            return null;
        }

        public bool ContainsKey(int rango)
        {
            for (int i = 0; i < this.Count; i++)
            {
                if (this[i].Key == rango)
                {
                    return true;
                }
            }
            return false;
        }

        public void Add(int rango, ConfiguracionPurebaPID configuracionPurebaPID)
        {
            GainPID keyValue = new GainPID(rango, configuracionPurebaPID);
            this.Add(keyValue);

        }

        public void Remove(int rango)
        {
            for (int i = 0; i < this.Count; i++)
            {
                if (this[i].Key == rango)
                {
                    this.RemoveAt(i);
                }
            }
        }
        /// <summary>
        /// Nos devuleve el tp del primer pid 
        /// </summary>
        /// <returns></returns>
        public double GetTp()
        {
            return this.ElementAt(0).Value.TPaso;
        }
    }


    [Serializable]
    public class GainPID
    {
        public int Key { get; set; }
        public ConfiguracionPurebaPID Value { get; set; }

        public GainPID() { }

        public GainPID(int key,ConfiguracionPurebaPID pid)
        {
            this.Key = key;
            this.Value = pid;
        }


    }
}
